New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

SIH: Publish Aux Global Positon #24051

Open

bresch wants to merge 5 commits into main from pr-sih_agp

+421 −0

Member

bresch commented Nov 28, 2024

Solved Problem

I need to test Aux Global Position (AGP) in sim.

Solution

Implement SensorAgpSim to publish AGP based on groundtruth data.

bresch added the Sim: SITL label

bresch requested a review from dagar

November 28, 2024 15:35

bresch self-assigned this

github-actions bot commented Nov 28, 2024 •

edited

Loading

🔎 FLASH Analysis

px4_fmu-v5x [Total VM Diff: 8 byte (0 %)]

    FILE SIZE        VM SIZE    
--------------  -------------- 
+0.0%      +8  +0.0%      +8    .text
  +0.0%      +5  +0.0%      +5    ROMFS/nsh_romfsimg.c
  +0.0%      +3  +0.0%      +3    [section .text]
+0.0%     +56  [ = ]       0    .debug_abbrev
   +11%     +56  [ = ]       0    ../../src/lib/version/version.c
-0.0%      -8  [ = ]       0    .debug_aranges
  -5.0%      -8  [ = ]       0    ../../src/lib/version/version.c
-0.0%      -4  [ = ]       0    .debug_info
  -0.2%      -4  [ = ]       0    ../../src/lib/version/version.c
-0.0%     -28  [ = ]       0    .debug_line
  -1.3%     -25  [ = ]       0    ../../src/lib/version/version.c
  -0.3%      -3  [ = ]       0    task/task_cancelpt.c
-0.0%      -8  [ = ]       0    .debug_ranges
  -2.6%      -8  [ = ]       0    ../../src/lib/version/version.c
-0.1%      -8  [ = ]       0    [Unmapped]
+0.0%      +8  +0.0%      +8    TOTAL

px4_fmu-v6x [Total VM Diff: 16 byte (0 %)]

    FILE SIZE        VM SIZE    
--------------  -------------- 
+0.0%     +16  +0.0%     +16    .text
  +0.0%      +8  +0.0%      +8    [section .text]
  +0.0%      +5  +0.0%      +5    ROMFS/nsh_romfsimg.c
  +0.2%      +3  +0.2%      +3    ../../src/systemcmds/ver/ver.cpp
+0.0%     +56  [ = ]       0    .debug_abbrev
   +11%     +56  [ = ]       0    ../../src/lib/version/version.c
-0.0%      -8  [ = ]       0    .debug_aranges
  -5.0%      -8  [ = ]       0    ../../src/lib/version/version.c
-0.0%      -4  [ = ]       0    .debug_info
  -0.2%      -4  [ = ]       0    ../../src/lib/version/version.c
-0.0%     -28  [ = ]       0    .debug_line
  -1.3%     -25  [ = ]       0    ../../src/lib/version/version.c
  -0.3%      -3  [ = ]       0    task/task_cancelpt.c
-0.0%      -8  [ = ]       0    .debug_ranges
  -2.6%      -8  [ = ]       0    ../../src/lib/version/version.c
-0.0%     -16  [ = ]       0    [Unmapped]
+0.0%      +8  +0.0%     +16    TOTAL

Updated: 2024-12-10T14:27:34

bresch force-pushed the pr_ekf2_ellipsoidal_sih branch from 3b401e2 to a984d0d Compare

November 29, 2024 12:11

Base automatically changed from pr_ekf2_ellipsoidal_sih to main

November 29, 2024 13:21

bresch added 3 commits

December 6, 2024 14:58


          add SensorAgpSim to generate Aux Global Position data

b4ddaad


          SIH-AGP: add sensor failure simulation


          ekf2_replay: log groundtruth messages

723fd7d

bresch force-pushed the pr-sih_agp branch from ef7654a to 723fd7d Compare

December 6, 2024 14:56

bresch marked this pull request as ready for review

December 6, 2024 15:08

bresch added 2 commits

December 10, 2024 13:49


          sih-agp: do not automatically enable AGP sensor

908fe66


          Merge branch 'main' into pr-sih_agp

1c87ea3

sfuhrer reviewed

View reviewed changes

src/modules/simulation/sensor_agp_sim/parameters.c

+               *
+               ****************************************************************************/
+              /**
+               * Simulate Aux Global Position (AGP)

Contributor

sfuhrer Dec 12, 2024

Suggested change

      
             * Simulate Aux Global Position (AGP)
          
             * Simulate Aux Global Position (AGP) in SIH

src/modules/simulation/sensor_agp_sim/parameters.c

+              PARAM_DEFINE_INT32(SENS_EN_AGPSIM, 0);
+              /**
+               * AGP failure mode

Contributor

sfuhrer Dec 12, 2024

Suggested change

      
             * AGP failure mode
          
             * AGP failure mode (in SIH)

Contributor

sfuhrer Dec 12, 2024

That param could use some more description.

Member Author

bresch Dec 12, 2024

Hmm, I think the original objective for this simulation module is to be re-used in multiple simulators, not only SIH.

src/modules/simulation/sensor_agp_sim/SensorAgpSim.cpp

@@ @@ -0,0 +1,202 @@ @@
+              /****************************************************************************
+               *
+               *   Copyright (c) 2021 PX4 Development Team. All rights reserved.

Contributor

sfuhrer Dec 12, 2024

Suggested change

      
             *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
          
             *   Copyright (c) 2024 PX4 Development Team. All rights reserved.

src/modules/simulation/sensor_agp_sim/SensorAgpSim.cpp

+              	if (phase) {
+              		do {
+              			float U1 = (float)rand() / (float)RAND_MAX;

Contributor

sfuhrer Dec 12, 2024

Out of curiosity, why did you change from the static cast used in BlockRandGauss.hpp to the regular cast?

Member Author

bresch Dec 12, 2024

I dumbly copy-pasted the SensorGpsSim class.
I think it requires some refactoring as it's nonsense to re-declare that function in every class. I don't understand if there is was a real reason to not have a common lib.

src/modules/simulation/sensor_agp_sim/SensorAgpSim.cpp

+              			_position_bias.zero();
+              		}
+              		double latitude = _measured_lla.latitude_deg() + math::degrees((double)generate_wgn() * 2.0 /

Contributor

sfuhrer Dec 12, 2024

Suggested change

      
            		double latitude = _measured_lla.latitude_deg() + math::degrees((double)generate_wgn() * 2.0 /
          
            		const double latitude = _measured_lla.latitude_deg() + math::degrees((double)generate_wgn() * 2.0 /

Contributor

sfuhrer Dec 12, 2024

and below

src/modules/simulation/sensor_agp_sim/SensorAgpSim.cpp

+              	PRINT_MODULE_DESCRIPTION(
+              		R"DESCR_STR(
+              ### Description

Contributor

sfuhrer Dec 12, 2024

Suggested change

      
            Module to simulate auxiliary global position measurements with optional failure modes for SIH simulation.

src/modules/simulation/sensor_agp_sim/SensorAgpSim.cpp

+              				  CONSTANTS_RADIUS_OF_EARTH);
+              		double longitude = _measured_lla.longitude_deg() + math::degrees((double)generate_wgn() * 2.0 /
+              				   CONSTANTS_RADIUS_OF_EARTH);
+              		double altitude = (double)(_measured_lla.altitude() + (generate_wgn() * 0.5f));

Contributor

sfuhrer Dec 12, 2024

How did you come up with the scales for the noise? Maybe worth some comments. And should we maybe define the scales somewhere centrally and then also scale .eph and .epv accordingly? Such that you can simulate easily with different noise levels.

Member Author

bresch Dec 12, 2024

Yes, maybe as parameters? So we can then change the noise level during the simulation.

src/modules/simulation/sensor_agp_sim/parameters.c

+               * @group Simulator
+               * @min 0
+               * @max 3
+               * @bit 0 Constant

Contributor

sfuhrer Dec 12, 2024

How about calling it "Measurement stuck "?

Member Author

bresch Dec 12, 2024

Yes, I wasn't sure if "frozen", "stuck" or "constant" would be more appropriate; I can change it to "stuck" if you find it clearer

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels